
#include "PulleyJoint.h"
#include "World.h"

namespace p2
{

IMPLEMENT_RTT(p2, PulleyJoint);

PulleyJoint::~PulleyJoint()
{
}

PulleyJoint* PulleyJoint::create(Anchor const& a0, Anchor const& pA, real length, bool swapOrder, void* userData)
{
	PulleyJoint* newPulleyJoint = _allocator.allocate(1);
	if (newPulleyJoint) {
		_allocator.construct(newPulleyJoint, PulleyJoint(pA, a0, length, swapOrder, userData));
	}
	return newPulleyJoint;
}

PulleyJoint* PulleyJoint::create(Anchor const& a0, Anchor const& pA, Anchor const& a1, real length, bool swapOrder, void* userData)
{
	PulleyJoint* newPulleyJoint = _allocator.allocate(1);
	if (newPulleyJoint) {
		_allocator.construct(newPulleyJoint, PulleyJoint(pA, a0, a1, length, swapOrder, userData));
	}
	return newPulleyJoint;
}

/*
 * computeTangent:
 * computes a point on the tangent of a circle centered at (0, 0) and radius r which passes through a point outside the circumference
 * working:
 *
 */
Vector2<real> PulleyJoint::computeTangentPoint(Vector2<real> x, real r, real order)
{
	real r2 = r*r;
    real xx2 = x[0] * x[0];
    real xy2 = x[1] * x[1];
	real rxx = (r2*x[0] + order*r*x[1]*::sqrtf(xx2-r2+xy2)) / (xx2+xy2);
	real rxy = order * ((x[0] < rxx) ? +::sqrtf(r2 - rxx*rxx) : -::sqrtf(r2 - rxx*rxx));
	return Vector2<real>(rxx, rxy);
}

void PulleyJoint::computeFutureAnchorPositions(real64 dT)
{
	/*
	 *
	 */
    if ( _pA.getEntity() ) {
    	_xPth = _pA.getEntity()->getX() + _pA.getEntity()->getV() * (real)dT;
    } else {
    	_xPth = _anchor0.getPoint();
    }

    if ( _anchor0.getEntity() ) {
    	_xAth = _anchor0.getEntity()->getX() + _anchor0.getEntity()->getV() * (real)dT;
    	Matrix2<real> rot;
    	real aAth = _anchor0.getEntity()->getA() + _anchor0.getEntity()->getW() * (real)dT;
        rot.makeRotation(aAth);
        _rAth = _anchor0.getPoint() * rot;
    } else {
    	_xAth = _anchor0.getPoint();
        _rAth = Vector2<real>::ZERO;
    }
    Vector2<real> xPAth = ((_xAth + _rAth) - _xPth);
	_rPAth = Vector2<real>(_xPth[0]-_radius, _xPth[1]);//PulleyJoint::computeTangentPoint(xPAth, _radius, +_order);

	if ( _anchor1.getEntity() != _pA.getEntity()) {
    	if ( _anchor1.getEntity() ) {
			_xBth = _anchor1.getEntity()->getX() + _anchor1.getEntity()->getV() * (real)dT;
			Matrix2<real> rot;
			real aBth = _anchor1.getEntity()->getA() + _anchor1.getEntity()->getW() * (real)dT;
			rot.makeRotation(aBth);
			_rBth = _anchor1.getPoint() * rot;
		} else {
			_xBth = _anchor1.getPoint();
			_rBth = Vector2<real>::ZERO;
		}
		Vector2<real> xPBth = ((_xBth + _rBth) - _xPth);
		_rPBth = Vector2<real>(_xPth[0]+_radius, _xPth[1]);//PulleyJoint::computeTangentPoint(xPBth, _radius, -_order);
    }
}

void PulleyJoint::preStep(real64 dT)
{
	Constraint::preStep(dT);
	computeFutureAnchorPositions(dT);
}

bool PulleyJoint::solvePositions(real64 dT)
{
    /*
     * REF: http://i31www.ira.uka.de/docs/Paper_VC05.pdf
     * 'Now the distance vector d between the two points at time t0 + h can be determined:
     * d(t0 + h) = B(t0 + h) - A(t0 + h)'
     */
	Vector2<real> npauth = _xPth + _rPAth - _xAth - _rAth;
    real dpath = npauth.length();
	Vector2<real> npbuth = _xPth + _rPAth - _xBth - _rBth;
    real dpbth = npbuth.length();
    if ( dpath <= EPOS && dpbth <= EPOS ) {
        return true;
    }
    npauth /= dpath;
    npbuth /= dpath;

    _invKpa = Entity::invK(_anchor0.getEntity(), _pA.getEntity(), _rAth, _rPAth, npauth);
    _invKpb = Entity::invK(_anchor1.getEntity(), _pA.getEntity(), _rBth, _rPBth, npbuth);

    Vector2<real> impulse = Vector2<real>::ZERO;
//    if (_flags & FLAGS_ENABLE_SPRING) {
//        /*
//         * REF: http://gafferongames.com/game-physics/spring-physics/
//         * 'A spring-damper system can be modeled as follows: F = - kx - bv
//         * where b is the coefficient of damping and v is the relative velocity
//         * between the two points connected by the spring'
//         */
//        real dx = _length - dth;
//        real dv = -nuth.dot(Entity::relativeVelocity(_anchor0.getEntity(), _anchor1.getEntity(), _rAth, _rBth));
//        real F = - _k * dx - _b * dv;
//        real mag = F * (real)dT * _invK;
//        impulse = mag * nuth;
//    } else {
		/*
		 * REF: http://i31www.ira.uka.de/docs/Paper_VC05.pdf
		 * 'An impulse P that eliminates the distance d in a time step of length h must change
		 * the relative velocity of the two points by d / h.'
		 */
		real d = Math<real>::Clamp((dpath - _length), 0.f, +MAX_DISTANCE_CORRECTION);
		real dv = d / (real)dT;
		real remove = dv;
		real mag = remove * _invKpa;
		impulse = mag * npauth;
//    }

    /*
     * REF: http://electron6.phys.utk.edu/101/CH2/angular_momentum.htm
     * apply impulses
     */
//	if ( _anchor0.getEntity() ) {
//		PositionVelocity& e0 = _owner->getSolverMap().at(_anchor0.getEntity());
//		e0.incV(+_anchor0.getEntity()->getInvMass() * impulse);
//		e0.incW(+_anchor0.getEntity()->getInvInertia() * _rAth.perp().dot(impulse));
//	}
//	if ( _pA.getEntity() ) {
//		PositionVelocity& pa = _owner->getSolverMap().at(_pA.getEntity());
//		pa.incV(-_pA.getEntity()->getInvMass() * impulse);
//		pa.incW(-_pA.getEntity()->getInvInertia() * _rPAth.perp().dot(impulse));
//	}

    return false;
}

bool PulleyJoint::solveVelocities(real64 dT)
{
	UNUSED(dT);
    /*
     * REF: http://i31www.ira.uka.de/docs/Paper_VC05.pdf
     * 'With the velocity difference dut = (uBt - uAt) and the matrix
     * K = (Ka + Kb) from the last section an impulse P can
     * be computed which eliminates the velocity difference if it is
     * applied to the points in opposite directions'
     */
//    Vector2<real> dut = (e1 ? (e1->getV()+_dv[1].getV()) + (e1->getW()+_dv[1].getW()) * _rBth.perp() : Vector2<real>::ZERO)
//    		- (e0 ? (e0->getV()+_dv[0].getV()) + (e0->getW()+_dv[0].getW()) * _rAth.perp() : Vector2<real>::ZERO);
//    if ( dut <= EVEL ) {
//        return true;
//    }

	/*
	 * eliminate velocity difference dut
	 */
//    real remove = Math<real>::Clamp(dut, 0.f, +MAX_VELOCITY_CORRECTION);
//    real mag = remove * _invKpa;
//    applyImpulses(mag * _uth);

    return false;
}

} /* namespace p2 */
